﻿
// 2018/8/21: 首个记录，基于V4 2018/8/14版本
// 2018/10/23: FromGeneralSample支持不带轨迹的样本
// 2018/11/29: 增加Protocols静态属性
// 2019/3/12: 修正LC/RC指标计算问题
// 2019/3/22: 时间戳赋值
// 2019/3/27: ToGeneralSample协议赋值
// 2019/3/28: 修正朝向插值问题
// 2019/3/29: 添加注释
// 2019/7/5: 修正RC计算bug
// 2019/7/31: 修正BoxCorners计算bug
// 2019/8/1: 修正BoxCorners计算bug Math计算时输入误用为角度
// 2019/9/27: 扩充目标物类别
// 2019/12/25: 添加若干目标物种类

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ASEva;

namespace ObjSensorSampleV5
{
    enum ObjectClass
    {
        General = 1, // 一般物体大类
        Car = 2, // 车辆大类
        Pedestrian = 3, // 行人大类
        Truck = 4, // 货车大类
        Bike = 5, // Bike大类
        RailCar = 6, // 轨道车辆大类
        Special = 7, // 特殊物体大类
        RoadStatic = 8, // 道路内静态物体大类
        SideStatic = 9, // 道路外静态物体大类

        GeneralSmall = 11, // 一般小物体
        GeneralBig = 12, // 一般大物体
        Barrier = 13, // 一般障碍物

        Van = 21, // 面包车
        Minibus = 22, //小巴
        Bus = 23, // 大巴
        BatteryCart = 24, // 园区电瓶车
        TinyCar = 25, // 微型车
        SUV = 26, // SUV

        Adult = 31, // 成人
        Child = 32, // 小孩
        Scooter = 33, // 平衡车
        WheelChair = 34, // 轮椅

        Minitruck = 41, // 小卡车
        ContainerTruck = 42, // 货柜车
        SpecialCar = 43, // 特种车辆
        Trailer = 44, // 拖车

        Motorbike = 51, // 摩托车
        Bicycle = 52, // 自行车
        ElectricBike = 53, // 电瓶自行车
        Tricycle = 54, // 三轮车

        Train = 61, // 火车
        Tram = 62, // 有轨电车

        Animal = 71, // 动物
        Ball = 72, // 球类
        Litter = 73, // 垃圾等杂物

        Cone = 81, // 锥形路障
        ManholeCover = 82, // 井盖
        Patch = 83, // 路面补丁
        Gantry = 84, // 龙门架

        Pole = 91, // 竖杆
        Tree = 92, // 树木
        Vegetation = 93, // 灌木
        Building = 94, // 建筑物
    }

    enum PositionMode
    {
        ClosestPoint = 1, // 最近点
        BoxCenter = 2, // 框中心
    }

    class ObjectInfo
    {
        // 主要部分
        public int ID { get; set; } // 目标物ID
        public int Age { get; set; } // 目标物的Age
        public int? PublicID { get; set; } // 目标物的原始ID
        public int? PublicAge { get; set; } // 目标物的原始Age

        public int? TimeOffset { get; set; } // 目标物的时间偏置 us
        public Color? Color { get; set; } // 目标物的颜色

        public ObjectClass ObjectClass { get; set; } // 目标物的类型
        public int? ObjectClassID { get; set; } // 目标物的类别ID
        public double? ObjectClassConfidence { get; set; } // 目标物的分类置信度 %

        public PositionMode PosMode { get; set; } // 目标物的位置模式
        public double PosX { get; set; } // 目标物的x轴方向位置 m
        public double PosY { get; set; } // 目标物的y轴方向位置 m
        public double? PosXSigma { get; set; } // 目标物x轴方向位置的精度 m
        public double? PosYSigma { get; set; } // 目标物y轴方向位置的精度 m

        public double CPX { get; set; } // 目标物的最近点x轴坐标 m
        public double CPY { get; set; } // 目标物的最近点y轴坐标 m
        public double CPD { get; set; } // 目标物的最近点与本车轮廓距离 m

        public double? VXAbs { get; set; } // x轴方向绝对速度 KPH
        public double? VYAbs { get; set; } // y轴方向绝对速度 KPH
        public double? AXAbs { get; set; } // x轴方向绝对加速度 m/s²
        public double? AYAbs { get; set; } // y轴方向绝对加速度 m/s²
        public double? VXRel { get; set; } // x轴方向相对速度 KPH
        public double? VYRel { get; set; } // y轴方向相对速度 KPH
        public double? AXRel { get; set; } // x轴方向相对加速度 m/s²
        public double? AYRel { get; set; } // y轴方向相对加速度 m/s²

        public double? Heading { get; set; } // 目标物的朝向 deg
        public double? Width { get; set; } // 目标物的宽度 m
        public double? Length { get; set; } // 目标物的长度 m
        public PointF[] Trajectory { get; set; } // 目标物轨迹点的列表
        public PointF[] Contour { get; set; } // 目标物轮廓点的列表

        // 指标部分
        public double? TTC1 { get; set; } // 一阶TTC s
        public double? TTC2 { get; set; } // 二阶TTC s
        public double? THW { get; set; } // 车间时距 s
        public double? PDCA { get; set; } // 潜在避免碰撞最小减速度 m/s² -一秒后再减速
        public double? ODCA { get; set; } // 显式避免碰撞最小减速度 m/s² -当前立即开始减速
        public double? Overlap { get; set; } // 重叠率 %
        public double? LC { get; set; } // 横向间距 m
        public double? RC { get; set; } // 后向间距 m

        public PointF ObjectPosition
        {
            get { return new PointF((float)PosX, (float)PosY); }
        }
        public PointF ClosestPoint
        {
            get { return new PointF((float)CPX, (float)CPY); }
        }
        public double? Speed
        {
            get
            {
                if (VXAbs != null && VYAbs != null) return Math.Sqrt(VXAbs.Value * VXAbs.Value + VYAbs.Value * VYAbs.Value);
                else return null;
            }
        }

        public ObjectInfo()
        {
            PosMode = PositionMode.ClosestPoint;
            ObjectClass = ObjectClass.General;
        }
    }

    class FovDescription
    {
        public double PositionX { get; set; } // FOV中心点x轴坐标 m
        public double PositionY { get; set; } // FOV中心的y轴坐标 m
        public double AngleRange { get; set; } // FOV的角度范围 deg
        public double Orientation { get; set; } // FOV中轴线朝向角 deg
        public double DistanceRange { get; set; } // FOV探测距离范围 m
        public double BlindRange { get; set; } // FOV盲区范围 m

        public FovDescription()
        {
            AngleRange = 90;
            DistanceRange = 100;
        }
    }

    class ObjectSensorSample : Sample
    {
        public List<ObjectInfo> Objects { get; set; } // 目标物列表
        public int? KeyObjectFrontIndex { get; set; } // 前向关键目标序号
        public int? KeyObjectLeftIndex { get; set; } // 左侧关键目标序号
        public int? KeyObjectRightIndex { get; set; } // 右侧关键目标序号

        public double? VehicleSpeed { get; set; } // 本车车速 KPH
        public double? VehicleCurvature { get; set; } // 本车曲率 1/m
        public double? VehicleWidth { get; set; } // 本车宽度 m
        public double? VehicleLength { get; set; } // 本车长度 m

        public List<FovDescription> Fovs { get; set; } // FOV列表

        public ObjectSensorSample()
        {
            Objects = new List<ObjectInfo>();
            Fovs = new List<FovDescription>();
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        public override string GetGeneralSampleProtocol()
        {
            return "obj-sensor-sample-v5";
        }

        public static string[] Protocols
        {
            get
            {
                return new string[] { "obj-sensor-sample-v3", "obj-sensor-sample-v4", "obj-sensor-sample-v5" };
            }
        }
        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.Base = Base;
            sample.Offset = Offset;
            sample.Timeline = Timeline;
            sample.Protocol = GetGeneralSampleProtocol();

            int trajectoryPoints = 0;
            int contourPoints = 0;
            var trajectoryOffset = new int[Objects.Count];
            var trajectorySize = new int[Objects.Count];
            var contourOffset = new int[Objects.Count];
            var contourSize = new int[Objects.Count];
            for (int i = 0; i < Objects.Count; i++)
            {
                trajectoryOffset[i] = trajectoryPoints;
                trajectorySize[i] = Objects[i].Trajectory == null ? 0 : Objects[i].Trajectory.Length;
                trajectoryPoints += trajectorySize[i];
                contourOffset[i] = contourPoints;
                contourSize[i] = Objects[i].Contour == null ? 0 : Objects[i].Contour.Length;
                contourPoints += contourSize[i];
            }

            var data = new GeneralSampleValue[11 + Objects.Count * 42 + Fovs.Count * 6 + (trajectoryPoints + contourPoints) * 2];
            int objBase = 11;
            int fovBase = objBase + Objects.Count * 42;
            int trajectoryBase = fovBase + Fovs.Count * 6;
            int contourBase = trajectoryBase + trajectoryPoints * 2;

            data[0] = new GeneralSampleValue((double)Objects.Count);
            data[1] = new GeneralSampleValue((double)trajectoryPoints);
            data[2] = new GeneralSampleValue((double)contourPoints);

            if (KeyObjectFrontIndex != null) data[3] = new GeneralSampleValue((double)KeyObjectFrontIndex.Value);
            if (KeyObjectLeftIndex != null) data[4] = new GeneralSampleValue((double)KeyObjectLeftIndex.Value);
            if (KeyObjectRightIndex != null) data[5] = new GeneralSampleValue((double)KeyObjectRightIndex.Value);

            if (VehicleSpeed != null) data[6] = new GeneralSampleValue(VehicleSpeed.Value);
            if (VehicleCurvature != null) data[7] = new GeneralSampleValue(VehicleCurvature.Value);
            if (VehicleWidth != null) data[8] = new GeneralSampleValue(VehicleWidth.Value);
            if (VehicleLength != null) data[9] = new GeneralSampleValue(VehicleLength.Value);

            data[10] = new GeneralSampleValue((double)Fovs.Count);

            for (int i = 0; i < Objects.Count; i++)
            {
                ObjectInfo src = Objects[i];
                int baseIndex = objBase + 42 * i;
                data[baseIndex + 0] = new GeneralSampleValue((double)src.ID);
                data[baseIndex + 1] = new GeneralSampleValue((double)src.Age);
                if (src.PublicID != null) data[baseIndex + 2] = new GeneralSampleValue((double)src.PublicID.Value);
                if (src.PublicAge != null) data[baseIndex + 3] = new GeneralSampleValue((double)src.PublicAge.Value);
                if (src.ObjectClassID != null) data[baseIndex + 4] = new GeneralSampleValue((double)src.ObjectClassID.Value);
                data[baseIndex + 5] = new GeneralSampleValue((double)(uint)src.ObjectClass);
                data[baseIndex + 6] = new GeneralSampleValue((double)(uint)src.PosMode);
                data[baseIndex + 7] = new GeneralSampleValue(src.PosX);
                data[baseIndex + 8] = new GeneralSampleValue(src.PosY);
                data[baseIndex + 9] = new GeneralSampleValue(src.CPX);
                data[baseIndex + 10] = new GeneralSampleValue(src.CPY);
                data[baseIndex + 11] = new GeneralSampleValue(src.CPD);
                if (src.Width != null) data[baseIndex + 12] = new GeneralSampleValue(src.Width.Value);
                if (src.Length != null) data[baseIndex + 13] = new GeneralSampleValue(src.Length.Value);
                if (src.Heading != null) data[baseIndex + 14] = new GeneralSampleValue(src.Heading.Value);
                if (src.VXRel != null) data[baseIndex + 15] = new GeneralSampleValue(src.VXRel.Value);
                if (src.VXAbs != null) data[baseIndex + 16] = new GeneralSampleValue(src.VXAbs.Value);
                if (src.VYRel != null) data[baseIndex + 17] = new GeneralSampleValue(src.VYRel.Value);
                if (src.VYAbs != null) data[baseIndex + 18] = new GeneralSampleValue(src.VYAbs.Value);
                if (src.AXRel != null) data[baseIndex + 19] = new GeneralSampleValue(src.AXRel.Value);
                if (src.AXAbs != null) data[baseIndex + 20] = new GeneralSampleValue(src.AXAbs.Value);
                if (src.AYRel != null) data[baseIndex + 21] = new GeneralSampleValue(src.AYRel.Value);
                if (src.AYAbs != null) data[baseIndex + 22] = new GeneralSampleValue(src.AYAbs.Value);
                if (src.TTC1 != null) data[baseIndex + 23] = new GeneralSampleValue(src.TTC1.Value);
                if (src.TTC2 != null) data[baseIndex + 24] = new GeneralSampleValue(src.TTC2.Value);
                if (src.THW != null) data[baseIndex + 25] = new GeneralSampleValue(src.THW.Value);
                if (src.PDCA != null) data[baseIndex + 26] = new GeneralSampleValue(src.PDCA.Value);
                if (src.ODCA != null) data[baseIndex + 27] = new GeneralSampleValue(src.ODCA.Value);
                if (src.Overlap != null) data[baseIndex + 28] = new GeneralSampleValue(src.Overlap.Value);
                if (src.LC != null) data[baseIndex + 29] = new GeneralSampleValue(src.LC.Value);
                if (src.RC != null) data[baseIndex + 30] = new GeneralSampleValue(src.RC.Value);
                data[baseIndex + 31] = new GeneralSampleValue((double)trajectoryOffset[i]);
                data[baseIndex + 32] = new GeneralSampleValue((double)trajectorySize[i]);
                data[baseIndex + 33] = new GeneralSampleValue((double)contourOffset[i]);
                data[baseIndex + 34] = new GeneralSampleValue((double)contourSize[i]);
                if (src.TimeOffset != null) data[baseIndex + 35] = new GeneralSampleValue(src.TimeOffset.Value);
                if (src.Color != null) data[baseIndex + 36] = new GeneralSampleValue(src.Color.Value.R);
                if (src.Color != null) data[baseIndex + 37] = new GeneralSampleValue(src.Color.Value.G);
                if (src.Color != null) data[baseIndex + 38] = new GeneralSampleValue(src.Color.Value.B);
                if (src.ObjectClassConfidence != null) data[baseIndex + 39] = new GeneralSampleValue(src.ObjectClassConfidence.Value);
                if (src.PosXSigma != null) data[baseIndex + 40] = new GeneralSampleValue(src.PosXSigma.Value);
                if (src.PosYSigma != null) data[baseIndex + 41] = new GeneralSampleValue(src.PosYSigma.Value);

                int dataOffset = trajectoryBase + trajectoryOffset[i] * 2;
                for (int n = 0; n < trajectorySize[i]; n++)
                {
                    var pt = src.Trajectory[n];
                    data[dataOffset + 2 * n] = new GeneralSampleValue(pt.X);
                    data[dataOffset + 2 * n + 1] = new GeneralSampleValue(pt.Y);
                }

                dataOffset = contourBase + contourOffset[i] * 2;
                for (int n = 0; n < contourSize[i]; n++)
                {
                    var pt = src.Contour[n];
                    data[dataOffset + 2 * n] = new GeneralSampleValue(pt.X);
                    data[dataOffset + 2 * n + 1] = new GeneralSampleValue(pt.Y);
                }
            }

            for (int i = 0; i < Fovs.Count; i++)
            {
                FovDescription src = Fovs[i];
                int baseIndex = fovBase + 6 * i;
                data[baseIndex + 0] = new GeneralSampleValue((double)src.PositionX);
                data[baseIndex + 1] = new GeneralSampleValue((double)src.PositionY);
                data[baseIndex + 2] = new GeneralSampleValue((double)src.Orientation);
                data[baseIndex + 3] = new GeneralSampleValue((double)src.AngleRange);
                data[baseIndex + 4] = new GeneralSampleValue((double)src.DistanceRange);
                data[baseIndex + 5] = new GeneralSampleValue((double)src.BlindRange);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = trajectoryBase;

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            #region obj-sensor-sample-v5
            if (sample.Protocol == "obj-sensor-sample-v5")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 11) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nTrajectories = (int)v[1].number;
                int nContours = (int)v[2].number;
                int nFovs = (int)v[10].number;

                int sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
                int sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
                if (v.Length != sizeWithExtra && v.Length != sizeWithoutExtra) return false;

                KeyObjectFrontIndex = v[3].ToInt();
                KeyObjectLeftIndex = v[4].ToInt();
                KeyObjectRightIndex = v[5].ToInt();

                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleWidth = v[8].ToDouble();
                VehicleLength = v[9].ToDouble();

                int objBase = 11;
                int fovBase = objBase + 42 * nObjects;
                int trajBase = fovBase + 6 * nFovs;
                int contBase = trajBase + 2 * nTrajectories;

                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = objBase + 42 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.PublicID = v[baseIndex + 2].ToInt();
                    obj.PublicAge = v[baseIndex + 3].ToInt();
                    obj.ObjectClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.CPX = v[baseIndex + 9].number;
                    obj.CPY = v[baseIndex + 10].number;
                    obj.CPD = v[baseIndex + 11].number;
                    obj.Width = v[baseIndex + 12].ToDouble();
                    obj.Length = v[baseIndex + 13].ToDouble();
                    obj.Heading = v[baseIndex + 14].ToDouble();
                    obj.VXRel = v[baseIndex + 15].ToDouble();
                    obj.VXAbs = v[baseIndex + 16].ToDouble();
                    obj.VYRel = v[baseIndex + 17].ToDouble();
                    obj.VYAbs = v[baseIndex + 18].ToDouble();
                    obj.AXRel = v[baseIndex + 19].ToDouble();
                    obj.AXAbs = v[baseIndex + 20].ToDouble();
                    obj.AYRel = v[baseIndex + 21].ToDouble();
                    obj.AYAbs = v[baseIndex + 22].ToDouble();
                    obj.TTC1 = v[baseIndex + 23].ToDouble();
                    obj.TTC2 = v[baseIndex + 24].ToDouble();
                    obj.THW = v[baseIndex + 25].ToDouble();
                    obj.PDCA = v[baseIndex + 26].ToDouble();
                    obj.ODCA = v[baseIndex + 27].ToDouble();
                    obj.Overlap = v[baseIndex + 28].ToDouble();
                    obj.LC = v[baseIndex + 29].ToDouble();
                    obj.RC = v[baseIndex + 30].ToDouble();
                    obj.TimeOffset = v[baseIndex + 35].ToInt();
                    if (v[baseIndex + 36].IsNumber()) obj.Color = Color.FromArgb((int)v[baseIndex + 36].number, (int)v[baseIndex + 37].number, (int)v[baseIndex + 38].number);
                    obj.ObjectClassConfidence = v[baseIndex + 39].ToDouble();
                    obj.PosXSigma = v[baseIndex + 40].ToDouble();
                    obj.PosYSigma = v[baseIndex + 41].ToDouble();

                    int trajOffset = (int)v[baseIndex + 31].number;
                    int trajSize = (int)v[baseIndex + 32].number;
                    int contOffset = (int)v[baseIndex + 33].number;
                    int contSize = (int)v[baseIndex + 34].number;

                    if (v.Length == sizeWithExtra)
                    {
                        obj.Trajectory = new PointF[trajSize];
                        for (int n = 0; n < trajSize; n++)
                        {
                            obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                            obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                        }

                        obj.Contour = new PointF[contSize];
                        for (int n = 0; n < contSize; n++)
                        {
                            obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                            obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                        }
                    }
                    else
                    {
                        obj.Trajectory = new PointF[0];
                        obj.Contour = new PointF[0];
                    }

                    Objects.Add(obj);
                }

                Fovs.Clear();
                for (int i = 0; i < nFovs; i++)
                {
                    var fov = new FovDescription();
                    int baseIndex = fovBase + 6 * i;

                    fov.PositionX = v[baseIndex].number;
                    fov.PositionY = v[baseIndex + 1].number;
                    fov.Orientation = v[baseIndex + 2].number;
                    fov.AngleRange = v[baseIndex + 3].number;
                    fov.DistanceRange = v[baseIndex + 4].number;
                    fov.BlindRange = v[baseIndex + 5].number;

                    Fovs.Add(fov);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            #region obj-sensor-sample-v4
            else if (sample.Protocol == "obj-sensor-sample-v4")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 16) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nTrajectories = (int)v[1].number;
                int nContours = (int)v[2].number;
                if (v.Length != 16 + nObjects * 35 + (nTrajectories + nContours) * 2) return false;

                KeyObjectFrontIndex = v[3].ToInt();
                KeyObjectLeftIndex = v[4].ToInt();
                KeyObjectRightIndex = v[5].ToInt();

                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleWidth = v[8].ToDouble();
                VehicleLength = v[9].ToDouble();

                double fovPosX = v[10].number;
                double fovPosY = v[11].number;
                double fovOrient = v[12].number;
                double fovAngleRange = v[13].number;
                double fovDistRange = v[14].number;
                bool fovSymmetry = v[15].number == 1;

                Fovs.Clear();
                Fovs.Add(new FovDescription()
                    {
                        PositionX = fovPosX,
                        PositionY = fovPosY,
                        Orientation = fovOrient,
                        AngleRange = fovAngleRange,
                        DistanceRange = fovDistRange,
                    });
                if (fovSymmetry)
                {
                    Fovs.Add(new FovDescription()
                        {
                            PositionX = fovPosX,
                            PositionY = -fovPosY,
                            Orientation = -fovOrient,
                            AngleRange = fovAngleRange,
                            DistanceRange = fovDistRange,
                        });
                }

                int trajBase = 16 + 35 * nObjects;
                int contBase = 16 + 35 * nObjects + 2 * nTrajectories;
                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = 16 + 35 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.PublicID = v[baseIndex + 2].ToInt();
                    obj.PublicAge = v[baseIndex + 3].ToInt();
                    obj.ObjectClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.CPX = v[baseIndex + 9].number;
                    obj.CPY = v[baseIndex + 10].number;
                    obj.CPD = v[baseIndex + 11].number;
                    obj.Width = v[baseIndex + 12].ToDouble();
                    obj.Length = v[baseIndex + 13].ToDouble();
                    obj.Heading = v[baseIndex + 14].ToDouble();
                    obj.VXRel = v[baseIndex + 15].ToDouble();
                    obj.VXAbs = v[baseIndex + 16].ToDouble();
                    obj.VYRel = v[baseIndex + 17].ToDouble();
                    obj.VYAbs = v[baseIndex + 18].ToDouble();
                    obj.AXRel = v[baseIndex + 19].ToDouble();
                    obj.AXAbs = v[baseIndex + 20].ToDouble();
                    obj.AYRel = v[baseIndex + 21].ToDouble();
                    obj.AYAbs = v[baseIndex + 22].ToDouble();
                    obj.TTC1 = v[baseIndex + 23].ToDouble();
                    obj.TTC2 = v[baseIndex + 24].ToDouble();
                    obj.THW = v[baseIndex + 25].ToDouble();
                    obj.PDCA = v[baseIndex + 26].ToDouble();
                    obj.ODCA = v[baseIndex + 27].ToDouble();
                    obj.Overlap = v[baseIndex + 28].ToDouble();
                    obj.LC = v[baseIndex + 29].ToDouble();
                    obj.RC = v[baseIndex + 30].ToDouble();

                    int trajOffset = (int)v[baseIndex + 31].number;
                    int trajSize = (int)v[baseIndex + 32].number;
                    int contOffset = (int)v[baseIndex + 33].number;
                    int contSize = (int)v[baseIndex + 34].number;

                    obj.Trajectory = new PointF[trajSize];
                    for (int n = 0; n < trajSize; n++)
                    {
                        obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                        obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                    }

                    obj.Contour = new PointF[contSize];
                    for (int n = 0; n < contSize; n++)
                    {
                        obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                        obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                    }

                    Objects.Add(obj);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            #region obj-sensor-sample-v3
            else if (sample.Protocol == "obj-sensor-sample-v3")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 7) return false;
                if (v[0].IsNotNumber() || v[0].number != sample.Channel.Value) return false;

                if (v[1].IsNotNumber() ||
                    v[2].IsNotNumber() ||
                    v[3].IsNotNumber()) return false;

                int nObjects = (int)v[1].number;
                int nTrajectories = (int)v[2].number;
                int nContours = (int)v[3].number;
                if (v.Length != 7 + nObjects * 33 + (nTrajectories + nContours) * 2) return false;

                KeyObjectFrontIndex = v[4].ToInt();
                KeyObjectLeftIndex = v[5].ToInt();
                KeyObjectRightIndex = v[6].ToInt();

                int trajBase = 7 + 33 * nObjects;
                int contBase = 7 + 33 * nObjects + 2 * nTrajectories;
                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    obj.ID = (int)v[33 * i + 7].number;
                    obj.Age = (int)v[33 * i + 8].number;
                    obj.ObjectClassID = v[33 * i + 9].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[33 * i + 10].number;
                    obj.PosMode = (PositionMode)(int)v[33 * i + 11].number;
                    obj.PosX = v[33 * i + 12].number;
                    obj.PosY = v[33 * i + 13].number;
                    obj.CPX = v[33 * i + 14].number;
                    obj.CPY = v[33 * i + 15].number;
                    obj.Width = v[33 * i + 17].ToDouble();
                    obj.Length = v[33 * i + 18].ToDouble();
                    obj.Heading = v[33 * i + 19].ToDouble();
                    obj.VXRel = v[33 * i + 20].ToDouble();
                    obj.VXAbs = v[33 * i + 21].ToDouble();
                    obj.VYRel = v[33 * i + 22].ToDouble();
                    obj.VYAbs = v[33 * i + 23].ToDouble();
                    obj.AXRel = v[33 * i + 24].ToDouble();
                    obj.AXAbs = v[33 * i + 25].ToDouble();
                    obj.AYRel = v[33 * i + 26].ToDouble();
                    obj.AYAbs = v[33 * i + 27].ToDouble();
                    obj.TTC1 = v[33 * i + 28].ToDouble();
                    obj.TTC2 = v[33 * i + 29].ToDouble();
                    obj.THW = v[33 * i + 30].ToDouble();
                    obj.PDCA = v[33 * i + 31].ToDouble();
                    obj.ODCA = v[33 * i + 32].ToDouble();
                    obj.Overlap = v[33 * i + 33].ToDouble();
                    obj.LC = v[33 * i + 34].ToDouble();
                    obj.RC = v[33 * i + 35].ToDouble();

                    int trajOffset = (int)v[33 * i + 36].number;
                    int trajSize = (int)v[33 * i + 37].number;
                    int contOffset = (int)v[33 * i + 38].number;
                    int contSize = (int)v[33 * i + 39].number;

                    obj.Trajectory = new PointF[trajSize];
                    for (int n = 0; n < trajSize; n++)
                    {
                        obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                        obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                    }

                    obj.Contour = new PointF[contSize];
                    for (int n = 0; n < contSize; n++)
                    {
                        obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                        obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                    }

                    Objects.Add(obj);
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            #endregion

            return false;
        }

        protected override Sample Interpolate(SearchResult input)
        {
            var s1 = input.s1 as ObjectSensorSample;
            var s2 = input.s2 as ObjectSensorSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new ObjectSensorSample();

            // Vehicle, FOV
            output.VehicleWidth = s1.VehicleWidth;
            output.VehicleLength = s1.VehicleLength;
            output.VehicleSpeed = s1.VehicleSpeed * w1 + s2.VehicleSpeed * w2;
            output.VehicleCurvature = s1.VehicleCurvature * w1 + s2.VehicleCurvature * w2;

            output.Fovs = s1.Fovs;

            int? koFrontID = null, koLeftID = null, koRightID = null;
            if (s1.KeyObjectFrontIndex != null && s2.KeyObjectFrontIndex != null &&
                s1.Objects[s1.KeyObjectFrontIndex.Value].ID == s2.Objects[s2.KeyObjectFrontIndex.Value].ID)
            {
                koFrontID = s1.Objects[s1.KeyObjectFrontIndex.Value].ID;
            }
            if (s1.KeyObjectLeftIndex != null && s2.KeyObjectLeftIndex != null &&
                s1.Objects[s1.KeyObjectLeftIndex.Value].ID == s2.Objects[s2.KeyObjectLeftIndex.Value].ID)
            {
                koLeftID = s1.Objects[s1.KeyObjectLeftIndex.Value].ID;
            }
            if (s1.KeyObjectRightIndex != null && s2.KeyObjectRightIndex != null &&
                s1.Objects[s1.KeyObjectRightIndex.Value].ID == s2.Objects[s2.KeyObjectRightIndex.Value].ID)
            {
                koRightID = s1.Objects[s1.KeyObjectRightIndex.Value].ID;
            }

            output.Objects = new List<ObjectInfo>();
            int index = 0;
            foreach (var o1 in s1.Objects)
            {
                var id = o1.ID;
                foreach (var o2 in s2.Objects)
                {
                    if (o2.ID != id) continue;

                    if (koFrontID != null && koFrontID.Value == id) output.KeyObjectFrontIndex = index;
                    if (koLeftID != null && koLeftID.Value == id) output.KeyObjectLeftIndex = index;
                    if (koRightID != null && koRightID.Value == id) output.KeyObjectRightIndex = index;

                    var newObj = new ObjectInfo();
                    newObj.ID = id;
                    newObj.Age = o1.Age;
                    newObj.PublicID = o1.PublicID;
                    newObj.PublicAge = o1.PublicAge;
                    newObj.TimeOffset = (int?)(o1.TimeOffset * w1 + o2.TimeOffset * w2);
                    newObj.Color = w1 > w2 ? o1.Color : o2.Color;
                    newObj.ObjectClass = w1 > w2 ? o1.ObjectClass : o2.ObjectClass;
                    newObj.ObjectClassID = w1 > w2 ? o1.ObjectClassID : o2.ObjectClassID;
                    newObj.ObjectClassConfidence = w1 > w2 ? o1.ObjectClassConfidence : o2.ObjectClassConfidence;
                    newObj.PosMode = o1.PosMode;
                    newObj.PosX = o1.PosX * w1 + o2.PosX * w2;
                    newObj.PosY = o1.PosY * w1 + o2.PosY * w2;
                    newObj.PosXSigma = o1.PosXSigma * w1 + o2.PosXSigma * w2;
                    newObj.PosYSigma = o1.PosYSigma * w1 + o2.PosYSigma * w2;
                    newObj.CPX = o1.CPX * w1 + o2.CPX * w2;
                    newObj.CPY = o1.CPY * w1 + o2.CPY * w2;
                    newObj.CPD = o1.CPD * w1 + o2.CPD * w2;
                    newObj.VXAbs = o1.VXAbs * w1 + o2.VXAbs * w2;
                    newObj.VYAbs = o1.VYAbs * w1 + o2.VYAbs * w2;
                    newObj.AXAbs = o1.AXAbs * w1 + o2.AXAbs * w2;
                    newObj.AYAbs = o1.AYAbs * w1 + o2.AYAbs * w2;
                    newObj.VXRel = o1.VXRel * w1 + o2.VXRel * w2;
                    newObj.VYRel = o1.VYRel * w1 + o2.VYRel * w2;
                    newObj.AXRel = o1.AXRel * w1 + o2.AXRel * w2;
                    newObj.AYRel = o1.AYRel * w1 + o2.AYRel * w2;
                    newObj.Heading = interpolateAngle(o1.Heading, w1, o2.Heading, w2);
                    newObj.Width = o1.Width * w1 + o2.Width * w2;
                    newObj.Length = o1.Length * w1 + o2.Length * w2;
                    newObj.Trajectory = w1 > w2 ? o1.Trajectory : o2.Trajectory;
                    newObj.Contour = w1 > w2 ? o1.Contour : o2.Contour;

                    output.Objects.Add(newObj);
                    index++;
                }
            }

            output.RecalculateIndicators();

            return output;
        }

        public void RecalculateIndicators()
        {
            foreach (var obj in Objects)
            {
                //ttc1
                if (obj.VXRel.HasValue)
                {
                    double vxRelM = obj.VXRel.Value / 3.6;

                    if (obj.CPX > 0)
                    {
                        if (vxRelM >= -0.1) obj.TTC1 = 10000;
                        else obj.TTC1 = Math.Min(10000, -(obj.CPX / vxRelM));
                    }
                    else if (VehicleLength.HasValue && obj.CPX < -VehicleLength.Value)
                    {
                        if (vxRelM <= 0.1) obj.TTC1 = 10000;
                        else obj.TTC1 = Math.Min(10000, (-VehicleLength.Value - obj.CPX) / vxRelM);
                    }
                }
                else obj.TTC1 = null;

                //ttc2
                if (obj.VXRel.HasValue && obj.AXRel.HasValue)
                {
                    double vxRelM = obj.VXRel.Value / 3.6;
                    double axRel = obj.AXRel.Value;

                    if (obj.CPX > 0)
                    {
                        if (vxRelM >= -0.1) obj.TTC2 = 10000;
                        else if (Math.Abs(axRel) < 0.1) obj.TTC2 = Math.Min(10000, -(obj.CPX / vxRelM));
                        else if (vxRelM * vxRelM - 2 * axRel * obj.CPX >= 0) obj.TTC2 = Math.Min(10000, (-vxRelM - Math.Sqrt(vxRelM * vxRelM - 2 * axRel * obj.CPX)) / axRel); //0.5at^2 + vt - d = 0
                        else obj.TTC2 = 10000;
                    }
                    else if (VehicleLength.HasValue && obj.CPX < -VehicleLength.Value)
                    {
                        double dist = -VehicleLength.Value - obj.CPX;

                        if (vxRelM <= 0.1) obj.TTC2 = 10000;
                        else if (Math.Abs(axRel) < 0.1) obj.TTC2 = Math.Min(10000, (-VehicleLength.Value - obj.CPX) / vxRelM);
                        else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) obj.TTC2 = Math.Min(10000, (-vxRelM + Math.Sqrt(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
                        obj.TTC2 = 10000;
                    }
                    else obj.TTC2 = null;
                }
                else obj.TTC2 = null;

                //thw
                if (obj.CPX > 0 && VehicleSpeed.HasValue)
                {
                    double vxEgoM = VehicleSpeed.Value / 3.6;
                    if (vxEgoM <= 0.1) obj.THW = 10000;
                    else obj.THW = Math.Min(10000, obj.CPX / vxEgoM);
                }
                else obj.THW = null;

                //pdca
                obj.PDCA = calDCA(obj.CPX, VehicleSpeed, obj.VXAbs, obj.AXAbs, 1);

                //odca
                obj.ODCA = calDCA(obj.CPX, VehicleSpeed, obj.VXAbs, obj.AXAbs, 0);

                //overlap
                if (VehicleCurvature.HasValue && VehicleWidth.HasValue)
                {
                    double widthEgo = VehicleWidth.Value;
                    double curvEgo = VehicleCurvature.Value;

                    if (obj.PosX <= 0) obj.Overlap = null;
                    else
                    {
                        double widthObj;
                        if (obj.PosMode == PositionMode.ClosestPoint)    //ClosestPoint
                        {
                            switch (obj.ObjectClass)
                            {
                                case ObjectClass.Car:
                                    widthObj = 1.9;
                                    break;
                                case ObjectClass.Pedestrian:
                                    widthObj = 0.5;
                                    break;
                                default:
                                    widthObj = 1.0;
                                    break;
                            }
                        }
                        else  //BoxCenter
                        {
                            PointF[] corners = calBoxCorners(obj);
                            double miny = double.PositiveInfinity, maxy = double.NegativeInfinity;
                            for (int i = 0; i < corners.Length; i++)
                            {
                                miny = Math.Min(miny, corners[i].Y);
                                maxy = Math.Max(maxy, corners[i].Y);
                            }
                            widthObj = maxy - miny;
                        }

                        double dyEgo = 0.5 * curvEgo * obj.PosX * obj.PosX;
                        double dyObj = obj.PosY;
                        double overLap = ((widthEgo + widthObj) / 2) - Math.Abs(dyObj - dyEgo);
                        double overLapRate = overLap / widthEgo > 1 ? 1 : overLap / widthEgo < -3 ? -3 : overLap / widthEgo;
                        obj.Overlap = (int)(overLapRate * 100);
                    }
                }
                else obj.Overlap = null;

                //lc
                if (VehicleWidth == null) obj.LC = null;
                else if (obj.PosMode == PositionMode.ClosestPoint)
                {
                    if (obj.PosX > 0) obj.LC = null;
                    else if (obj.PosY > VehicleWidth.Value * 0.5) obj.LC = obj.PosY - VehicleWidth.Value * 0.5;
                    else if (obj.PosY < -VehicleWidth.Value * 0.5) obj.LC = -VehicleWidth.Value * 0.5 - obj.PosY;
                }
                else if (obj.PosMode == PositionMode.BoxCenter)
                {
                    PointF[] points = calBoxCorners(obj);

                    double minx = double.PositiveInfinity;
                    double miny = double.PositiveInfinity;
                    double maxy = double.NegativeInfinity;
                    for (int i = 0; i < points.Length; i++)
                    {
                        PointF pt = points[i];
                        if (pt.X < minx) minx = pt.X;
                        if (pt.Y < miny) miny = pt.Y;
                        if (pt.Y > maxy) maxy = pt.Y;
                    }

                    if (minx > 0) obj.LC = null;
                    else if (miny > 0) obj.LC = miny - VehicleWidth.Value * 0.5;
                    else if (maxy < 0) obj.LC = -VehicleWidth.Value * 0.5 - maxy;
                    else obj.LC = null;
                }
                else obj.LC = null;

                //rc
                if (VehicleLength == null) obj.RC = null;
                else
                {
                    PointF[] pts;
                    if (obj.PosMode == PositionMode.ClosestPoint) pts = new PointF[1] { new PointF((float)obj.PosX, (float)obj.PosY) };
                    else pts = calBoxCorners(obj);

                    double maxx = double.NegativeInfinity;
                    for (int i = 0; i < pts.Length; i++)
                    {
                        PointF pt = pts[i];
                        if (pt.X > maxx) maxx = pt.X;
                    }

                    if (maxx > 0) obj.RC = null;
                    else obj.RC = -VehicleLength.Value - maxx;
                }
            }
        }

        private double? calDCA(double cpx, double? vxego, double? vxabs, double? axabs, double reactionTime)
        {
            if (cpx > 0 && vxego.HasValue && vxabs.HasValue && axabs.HasValue)
            {
                double vxEgoM = vxego.Value / 3.6;
                double vxAbsM = vxabs.Value / 3.6;
                double axAbs = axabs.Value;

                double egoDxReaction = vxEgoM * reactionTime;
                double objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;

                double dxRelAR = cpx - 0.1 + objDxReaction - egoDxReaction;
                double egoVxAbsAR = vxEgoM;
                double objVxAbsAR = vxAbsM + axAbs * reactionTime;
                double objVxRelAR = vxAbsM - vxEgoM;
                double objAxAbsAR = axAbs;

                if (dxRelAR <= 0) return -10000;

                if (objAxAbsAR >= 0)
                {
                    if (objVxRelAR >= 0) return 0;
                    else return objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR);
                }
                else
                {
                    if (objVxRelAR >= 0) return egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR);
                    if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) return egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR);
                    else return objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR);
                }
            }
            else return null;
        }

        private PointF[] calBoxCorners(ObjectInfo obj)
        {
            double posx = obj.PosX;
            double posy = obj.PosY;
            double width = obj.Width.Value;
            double length = obj.Length.Value;
            double heading = obj.Heading.HasValue ? obj.Heading.Value : 0;

            PointF[] pts = new PointF[4];
            pts[0].X = (float)(length / 2); pts[0].Y = (float)(width / 2);
            pts[1].X = (float)(length / 2); pts[1].Y = (float)(-width / 2);
            pts[2].X = (float)(-length / 2); pts[2].Y = (float)(width / 2);
            pts[3].X = (float)(-length / 2); pts[3].Y = (float)(-width / 2);

            var deg2rad = Math.PI / 180;
            double coshd = Math.Cos(heading * deg2rad);
            double sinhd = Math.Sin(heading * deg2rad);
            for (int i = 0; i < 4; i++)
            {
                PointF pt = pts[i];
                double x = coshd * pt.X - sinhd * pt.Y;
                double y = sinhd * pt.X + coshd * pt.Y;
                pts[i].X = (float)(posx + x);
                pts[i].Y = (float)(posy + y);
            }
            return pts;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }
    }
}
